// Kevin Kemper

#include <fca_controllers/controller.h>

// macros to clean up the code
//#define	SINE_TIME		SIN_WAVE_CONTROLLER_STATE(state)->time

// controller constants
//#define K1		(130.4348)
//#define K2		(8.5652)

#define INVCB1	(0.0)
#define INVCB2	(-322.0612/1000)

//#define CABK11	(0.0)
//#define CABK12	(1.0000)
//#define CABK21	(-2.2500*1000)
//#define CABK22	(-165.000*1)


//#define KE1		(15.0000)
//#define KE2		(1.0000)

// Torque constant (Nm/Arms):
#define TC		0.18

// Gear ratio:
#define GR		17.25

#define A11		0
#define A12		1
#define A21		0
#define A22		-1

#define B1		0
#define B2		(-GR*TC)

#define C11		1
#define C12		0
#define C21		0
#define C22		1


//	A = [0, 1;
//		0, 0];
//	B = [0; G/I];
//	C = [1, 0;
//		0, 1];

/*	INPUT members:

	float			motor_angle;
	float			leg_angle;

	float			motor_velocity;
	float			leg_velocity;

	unsigned char	motor_current;

	unsigned char	command;
*/


//	e = yd(t) - C * x;
//	u = - K * x + (C * B) \ ( ydd(t) - C*(A - B*K)*x + Ke * e );
//	xdot = A * x + B *u;

//	yd = @(t) [ sin(t); cos(t) ];
//	ydd = @(t) -sin(t);


extern void initialize_position_sine_controller(ControllerInput *input, ControllerOutput *output, ControllerState *state, 
	ControllerData *data)
{
	output->motor_torque = 0.0;
	output->time = 0.0;
	POSITION_SINE_CONTROLLER_STATE(state)->time = 0.0;
}


extern void update_position_sine_controller(ControllerInput *input, ControllerOutput *output, ControllerState *state, ControllerData *data) {

	float spinrg_k		= 10.0;
	float spring_pos	= input->motor_angle - input->leg_angle;
	float spring_force	= spinrg_k * spring_pos;
	float t;
	
	float a		= POSITION_SINE_CONTROLLER_DATA(data)->pos_amp;
	float w		= POSITION_SINE_CONTROLLER_DATA(data)->pos_freq;
	float off	= POSITION_SINE_CONTROLLER_DATA(data)->pos_off;

	float k1	= -POSITION_SINE_CONTROLLER_DATA(data)->pos_k1;
	float k2	= -POSITION_SINE_CONTROLLER_DATA(data)->pos_k2;
	
	float ke1	= POSITION_SINE_CONTROLLER_DATA(data)->pos_ke1;
	float ke2	= POSITION_SINE_CONTROLLER_DATA(data)->pos_ke2;

	float err1,err2;
	float tmp1,tmp2;
	float bk11,bk12,bk21,bk22;
	float abk11,abk12,abk21,abk22;
	float cabk11,cabk12,cabk21,cabk22;

	float yd1, yd2;
	float ydd;
	
	float u;
	
	float x1 = input->motor_angle;
	float x2 = input->motor_velocity;
	
	POSITION_SINE_CONTROLLER_STATE(state)->time += 0.001;
	output->time = POSITION_SINE_CONTROLLER_STATE(state)->time;
		
	t	= POSITION_SINE_CONTROLLER_STATE(state)->time;


	yd1	= off+a*sin(w*t);
	yd2	= a*w*cos(w*t);
	ydd	= a*w*w*-sin(w*t);

	err1 = yd1 - (C11*x1 + C12*x2);
	err2 = yd2 - (C21*x1 + C22*x2);

	bk11 = B1*k1;
	bk12 = B1*k2;
	bk21 = B2*k1;
	bk22 = B2*k2;
	
	abk11 = A11-bk11;
	abk12 = A12-bk12;
	abk21 = A21-bk21;
	abk22 = A22-bk22;
	
	cabk11 = C11*abk11+C12*abk21;
	cabk12 = C11*abk12+C12*abk22;
	cabk21 = C21*abk11+C22*abk21;
	cabk22 = C21*abk12+C22*abk22;
	
	tmp1 = ydd - (cabk11*x1+cabk12*x2) + ke1*err1 + ke2*err2;
	tmp2 = ydd - (cabk21*x1+cabk22*x2) + ke1*err1 + ke2*err2;

	u = -(k1*x1+k2*x2) + INVCB1*tmp1 + INVCB2*tmp2;
	
//	if (u > 4.0)
//		u = 4.0;
//	else if (u < -4.0)
//		u = -4.0;
	
	output->motor_torque = (u - spring_force);
	
}


extern void takedown_position_sine_controller(ControllerInput *input, ControllerOutput *output, ControllerState *state, 
	ControllerData *data)
{
	output->motor_torque = 0.0;
	output->time = 0.0;
	POSITION_SINE_CONTROLLER_STATE(state)->time = 0.0;
}
